///////////////////////////////////////////////////////////////////////////////
//
// Lynxmotion SSC-32 Servo Controller Service
//
// Based on code from the Lynxmotion web site arm.1.2.zip
// Modified with the permission of Lynxmotion
//
// Updated Nov-2007 by Trevor Taylor, Software Technology
//
// This code is Open Source
//
// Contract Identifier and Namespace modified in Feb 2008 to avoid a conflict
// with the CoroWare ClassPack which also includes an SSC-32 service
//
///////////////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Dss.Core.Attributes;


namespace ProMRDS.Robotics.Lynxmotion.Ssc32
{
    [DataContract]
    public enum SSC32Commands
    {
        ServoMove = 1,
        PulseOffset = 13,
        DiscreteOutput = 2,
        ByteOutput = 3,
        QueryMovementStatus = 4,
        QueryPulseWidth = 5,
        ReadDigitalInputs = 6,
        ReadAnalogInputs = 7,
        HexapodSequencerCommands = 8,
        QueryHexSequencerState = 9,
        GetSoftwareVersion = 10,
        TransferToBoot = 11,
        SSCEmulation = 12,
    }

    public class SSC32Helper
    {
        //QueryPulseWidth command format:
        //QP <ch> \r
        //<ch> is the channel number

        //Returns a string of the QueryPulseWidth command
        //If the QueryPulseWidth command is invalid, a null string is returned
        public static string GenerateSSC32QueryPulseWidth(SSC32QueryPulseWidth command)
        {
            StringBuilder buff = new StringBuilder();

            for (int i = 0; i < command.Channels.Length && i < 32; i++)
            {
                //check that channels is between 0 and 31
                if (command.Channels[i] < 0 || command.Channels[i] > 31)
                    return null;

                buff.Append("QP " + command.Channels[i].ToString() + "\r ");
            }
            
            return buff.ToString();
        }

        //ServoMove command format:
        //# <ch> P <pw> S <spd> ...# <ch> P <pw> S <spd> T <time> \r
        //<ch> is the channel number
        //<pw> is the pulse width for the channel
        //<spd> is the speed for the channel (optional)
        //<time> is the time for the entire move (optional)

        //Returns a string of the ServoMove command
        //Returns a null string if the ServoMove command is invalid
        public static string GenerateSSC32ServoMove(SSC32ServoMove command)
        {
            StringBuilder buff = new StringBuilder();
            bool first = true;
            bool speed = false;

            //Check if Channels, PulseWidths, and Speeds are of the same length
            if (command.Channels.Length == command.PulseWidths.Length)
            {
                if (command.Speeds != null)
                {
                    speed = true;
                    if (command.Speeds.Length != command.Channels.Length)
                        return null;
                }
            }
            else
            {
                return null;
            }

            for (int i = 0; i < command.Channels.Length && i < 32; i++)
            {
                if (first)
                    first = false;
                else
                    buff.Append(' ');

                buff.Append('#');
                buff.Append(command.Channels[i].ToString());
                buff.Append(" P");
                buff.Append(command.PulseWidths[i].ToString());
                if (speed && command.Speeds[i] != -1)
                {
                    buff.Append(" S");
                    buff.Append(command.Speeds[i].ToString());
                }
            }

            if (command.Time != -1)
            {
                buff.Append(" T");
                buff.Append(command.Time.ToString());
            }

            buff.Append('\r');
            return buff.ToString();
        }
    }
}
